#include <QByteArray>
#include <QXmlAttributes>
#include <QXmlStreamReader>
#include <QString>
#include <QDebug>
#include <QDateTime>

#include "_Interface/_global_datastruct.h"
#include "backinstance.h"
#include "Package/oxfordlasersensordll_global.h"
#include "Package/simpletcpsocketdll_global.h"
#include "Package/SimpleRoboticMath/ConsoleMain/simpleroboticmath.h"

void  backinstance::onSeamCalibrationProcess_Auto(){
    if(this->WorkType == ENUM_WorkType::Auto){
        if(this->WorkModel == ENUM_WorkModel::Calibration){
            switch (this->pSeamFindingData->frRobotSeamScan) {
            case 11:
                if(this->pButtonStatus->CalPos1 != true){
                    this->onbtnClickedCalPos1();
                    this->pButtonStatus->CalPos1 = true;
                }
                break;
            case 12:
                if(this->pButtonStatus->CalPos2 != true){
                    this->onbtnClickedCalPos2();
                    this->pButtonStatus->CalPos2 = true;
                }
                break;
            case 13:
                if(this->pButtonStatus->CalPos3 != true){
                    this->onbtnClickedCalPos3();
                    this->pButtonStatus->CalPos3 = true;
                }
                break;
            case 14:
                if(this->pButtonStatus->CalPos4 != true){
                    this->onbtnClickedCalPos4();
                    this->pButtonStatus->CalPos4 = true;
                }
                break;
            case 15:
                if(this->pButtonStatus->CalPos5 != true){
                    this->onbtnClickedCalPos5();
                    this->pButtonStatus->CalPos5 = true;
                }
                break;
            case 16:
                if(this->pButtonStatus->CalPos6 != true){
                    this->onbtnClickedCalPos6();
                    this->pButtonStatus->CalPos6 = true;
                }
                break;
            case 17:
                if(this->pButtonStatus->CalibrationDo != true || this->pButtonStatus->CalibrationSave != true){
                    this->EyeHandsAotuCalibrationandSave();
                    this->pButtonStatus->CalibrationDo = true;
                    this->pButtonStatus->CalibrationSave = true;
                }
                break;
            }
        }
    }
}

void backinstance::EyeHandsAotuCalibrationandSave(){
    int i;
    int j;
    int k;

    double InerseMatrix[4][4];
    double InerseMatrix2[4][4];

    double Hand2Eyes1[4][4];
    double Hand2Eyes11[4][4];

    double Hand2Eyes2[4][4];
    double Hand2Eyes22[4][4];

    double Hand2Eyes3[4][4];
    double Hand2Eyes33[4][4];

    double Hand2Eyessum1[4][4];
    double Hand2Eyessum2[4][4];

    double tempPosXYZ[3];
    double tempEulerZYX[3];

    for(i = 0; i < 4; i++){
        for(j = 0; j < 4; j++){
            Hand2Eyes11[i][j] = 0;
            Hand2Eyes22[i][j] = 0;
            Hand2Eyes33[i][j] = 0;
        }
    }

    for(i = 0; i < 4; i++){
        Hand2Eyes11[i][i] = 1;
        Hand2Eyes22[i][i] = 1;
        Hand2Eyes33[i][i] = 1;
    }

    k = 0;

    if(this->Hand2EyesCalibrationData[0].bUsed == true && this->Hand2EyesCalibrationData[3].bUsed){
        d4MatrixInverse(this->Hand2EyesCalibrationData[0].mRobotMatrix,InerseMatrix);
        d4MatrixInverse(this->Hand2EyesCalibrationData[0].mLaserMatrix,InerseMatrix2);

        d4MatrixMultiply(InerseMatrix,this->Hand2EyesCalibrationData[3].mRobotMatrix,Hand2Eyes1);
        d4MatrixMultiply(Hand2Eyes1,InerseMatrix2,Hand2Eyes11);
        k = 1;
    }else{}

    if(this->Hand2EyesCalibrationData[1].bUsed == true && this->Hand2EyesCalibrationData[4].bUsed){
        d4MatrixInverse(this->Hand2EyesCalibrationData[1].mRobotMatrix,InerseMatrix);
        d4MatrixInverse(this->Hand2EyesCalibrationData[1].mLaserMatrix,InerseMatrix2);

        d4MatrixMultiply(InerseMatrix,this->Hand2EyesCalibrationData[4].mRobotMatrix,Hand2Eyes2);
        d4MatrixMultiply(Hand2Eyes2,InerseMatrix2,Hand2Eyes22);
        k = 2;
    }else{}

    if(this->Hand2EyesCalibrationData[2].bUsed == true && this->Hand2EyesCalibrationData[5].bUsed){
        d4MatrixInverse(this->Hand2EyesCalibrationData[2].mRobotMatrix,InerseMatrix);
        d4MatrixInverse(this->Hand2EyesCalibrationData[2].mLaserMatrix,InerseMatrix2);

        d4MatrixMultiply(InerseMatrix,this->Hand2EyesCalibrationData[5].mRobotMatrix,Hand2Eyes3);
        d4MatrixMultiply(Hand2Eyes3,InerseMatrix2,Hand2Eyes33);
        k = 3;
    }else{}

    if( k == 1){
        for(i = 0; i < 4; i++){
            for(j = 0; j < 4; j++){
                this->pHand2EyesMatrix->mMatrix[i][j] = Hand2Eyes11[i][j];
            }
        }
    }
    else if( k == 2){
        d4MatrixPlus(Hand2Eyes11,Hand2Eyes22,Hand2Eyessum1);
        for(i = 0; i < 4; i++){
            for(j = 0; j < 4; j++){
                this->pHand2EyesMatrix->mMatrix[i][j] = Hand2Eyessum1[i][j] / 2;
            }
        }
    }
    else if( k == 3){
        d4MatrixPlus(Hand2Eyes11,Hand2Eyes22,Hand2Eyessum1);
        d4MatrixPlus(Hand2Eyessum1,Hand2Eyes33,Hand2Eyessum2);
        for(i = 0; i < 4; i++){
            for(j = 0; j < 4; j++){
                this->pHand2EyesMatrix->mMatrix[i][j] = Hand2Eyessum2[i][j] / 3;
            }
        }
    }

    if(k != 0){

        d4Matrix2EulerZYX(this->pHand2EyesMatrix->mMatrix,tempPosXYZ,tempEulerZYX);

        this->pHand2EyesMatrix->x = tempPosXYZ[0];
        this->pHand2EyesMatrix->y = tempPosXYZ[1];
        this->pHand2EyesMatrix->z = tempPosXYZ[2];

        this->pHand2EyesMatrix->a = tempEulerZYX[0];
        this->pHand2EyesMatrix->b = tempEulerZYX[1];
        this->pHand2EyesMatrix->c = tempEulerZYX[2];

    }
    else{
        this->pHand2EyesMatrix->x = 0;
        this->pHand2EyesMatrix->y = 0;
        this->pHand2EyesMatrix->z = 0;

        this->pHand2EyesMatrix->a = 0;
        this->pHand2EyesMatrix->b = 0;
        this->pHand2EyesMatrix->c = 0;
    }
}

void backinstance::readAutoCalibrationResualt(double (&value)[6]){
    value[0] = this->pHand2EyesMatrix->x;
    value[1] = this->pHand2EyesMatrix->y;
    value[2] = this->pHand2EyesMatrix->z;

    value[3] = this->pHand2EyesMatrix->a;
    value[4] = this->pHand2EyesMatrix->b;
    value[5] = this->pHand2EyesMatrix->c;
}
